#include <Servo.h>
// 实例化6舵机
Servo servo_a;//定义舵机a
Servo servo_b;//定义舵机b
Servo servo_c;//定义舵机c
Servo servo_d;//定义舵机d
Servo servo_e;//定义舵机e
Servo servo_f;//定义舵机f

// 步进电机控制信号
//const int stepsPerRevolution = 1600;
#define stepsPerRevolution  1600  //步进电机转一圈所需脉冲数
#define LMT  A1      //步进电机限位开关引脚
#define DIR  A4     //步进电机方向
#define PUL  A5      //步进电机脉冲
#define EN   A0       //步进电机使能


//舵机引脚定义
#define sa  2 //左手上舵机
#define sb  3 //左手下舵机
#define sc  4 //左腿舵机
#define sd  5//右腿舵机
#define se  6 //右手下舵机
#define sf  7 //右手上舵机

//舵机位置误差补偿
int err_a=-3;
int err_b=-10;
int err_c=5;
int err_d=0;
int err_e=10;
int err_f=8;

//舵机初始位置
int pa=150; //左手上舵机
int pb=60; //左手下舵机
int pc=90; //左腿舵机
int pd=90; //右腿舵机
int pe=120; //右手下舵机
int pf=30; //右手上舵机

int xcount=0 ; //步进电机步数
int st= 0 ;
//int x=400；

//定义端口输入输出 初始化
void setup() {
  pinMode(DIR,OUTPUT);
  pinMode(PUL,OUTPUT);
  pinMode(EN,OUTPUT);
  pinMode(LMT,INPUT);
  servo_a.attach(sa); //定义舵机a控制接口
  servo_b.attach(sb); //定义舵机b控制接口
  servo_c.attach(sc); //定义舵机c控制接口
  servo_d.attach(sd); //定义舵机d控制接口
  servo_e.attach(se); //定义舵机e控制接口
  servo_f.attach(sf); //定义舵机f控制接口
  Serial.begin(19200);
  servo_a.write(pa+err_a);
  servo_b.write(pb+err_b);
  servo_c.write(pc+err_c);
  servo_d.write(pd+err_d);
  servo_e.write(pe+err_e);
  servo_f.write(pf+err_f);
}

void step_count(int c)
{
    int count=abs(c);
   for(int i=0;i<count;i++)
      if (c>0)
      {
        digitalWrite(DIR,HIGH);
        digitalWrite(PUL, HIGH);
        delayMicroseconds(100);
        digitalWrite(PUL, LOW);
        delayMicroseconds(100);
      }
      else if (c<0)
      {
        digitalWrite(DIR,LOW);
        digitalWrite(PUL, HIGH);
        delayMicroseconds(100);
        digitalWrite(PUL, LOW);
        delayMicroseconds(100);  
      }
}

void loop() { 
  if(Serial.available()){
    char cmd =Serial.read();
    if (cmd=='a') pa=Serial.parseInt(); 
    if (cmd=='b') pb=Serial.parseInt();
    if (cmd=='c') pc=Serial.parseInt();
    if (cmd=='d') pd=Serial.parseInt();
    if (cmd=='e') pe=Serial.parseInt();
    if (cmd=='f') pf=Serial.parseInt();
    if (cmd=='x') 
    {
     xcount=Serial.parseInt();
     step_count(xcount) ;
    }
    if (cmd=='s') st=Serial.parseInt();
  }
    servo_a.write(pa+err_a);
    servo_b.write(pb+err_b);
    servo_c.write(pc+err_c);
    servo_d.write(pd+err_d);
    servo_e.write(pe+err_e);
    servo_f.write(pf+err_f);
    digitalWrite(EN,st);
}
